#include "simulation/robot.h"

#include <Eigen3/Geometry>

#include <visualization_msgs/Marker.h>

Robot::Robot(float x, float y, float angle) :
	_x(x),
	_y(y),
	_angle(angle)
{
	ros::NodeHandle nh;
	marker_publisher = nh.advertise<visualization_msgs::Marker>("robot_marker", 1);
}

/**
 * Renders the robot by publishing marker messages to the /robot_marker topic.
 */
void Robot::render()
{
	// Find the rotation around the z-axis as a quaternion
	Eigen3::Quaternion<float> q(Eigen3::AngleAxis<float>(_angle, Eigen3::Vector3f(0.0f, 0.0f, 1.0f)));

	visualization_msgs::Marker marker;

	marker.header.frame_id = "/floor";
	marker.header.stamp = ros::Time::now();

	marker.ns = "robot_body";
	marker.id = 0;

	marker.type = visualization_msgs::Marker::CYLINDER;
	marker.action = visualization_msgs::Marker::ADD;

	//marker.mesh_resource = "package://simulation/models/robot.dae";

	marker.pose.position.x = _x;
	marker.pose.position.y = _y;
	marker.pose.position.z = 0.150;
	
	marker.pose.orientation.x = q.x();
	marker.pose.orientation.y = q.y();
	marker.pose.orientation.z = q.z();
	marker.pose.orientation.w = q.w();
	
	marker.scale.x = 0.22f;
	marker.scale.y = 0.22f;
	marker.scale.z = 0.3f;
	
	marker.color.r = 0.0;
	marker.color.g = 1.0;
	marker.color.b = 0.0;
	marker.color.a = 1.0;

	// Publish the cylinder
	marker_publisher.publish(marker);
	
	// Reuse the marker for the arrow
	marker.id = 1;
	marker.type = visualization_msgs::Marker::ARROW;

	marker.scale.x = 0.25f;
	marker.scale.y = 0.25f;
	marker.scale.z = 0.25f;

	marker.color.r = 1.0;
	marker.color.g = 0.0;
	marker.color.b = 0.0;
	marker.color.a = 1.0;

	// Publish the arrow
	marker_publisher.publish(marker);
}
